# *-* coding:utf8 *-*

import sys
from PyQt5.QtWidgets import QMainWindow ,QApplication,QMessageBox
from PyQt5.QtGui import QIcon
from matplotlib import colors
from numpy.lib.utils import safe_eval
from base import Ui_MainWindow
from mov1112 import Ui_MainWindow as mov_window

import matplotlib
# 使用 matplotlib中的FigureCanvas (在使用 Qt5 Backends中 FigureCanvas继承自QtWidge  ts.QWidget)
from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
from PyQt5 import QtCore, QtWidgets, QtGui
from PyQt5.QtWidgets import QDialog, QPushButton, QVBoxLayout
import matplotlib.pyplot as plt
import numpy as np
import sys

from env_train import robot_env

from new.env.world import WorldCreation

class movWindow(QMainWindow):
    def __init__(self,parent=None):
        super(movWindow,self).__init__(parent)
        self.ui = mov_window()
        self.ui.setupUi(self)
        self.setWindowTitle("策略仿真")
        self.ui.pushButton_2.clicked.connect(self.Draw_target)
        self.ui.pushButton_3.clicked.connect(self.Draw)
        self.ui.pushButton.clicked.connect(self.Draw_obstac)
        self.ui.pushButton_4.clicked.connect(self.play)
        self.ui.pushButton_5.clicked.connect(self.mov_the_robot_auto)
        self.simulation = WorldCreation()
        self.robotEnv = robot_env()



    def close(self):
        if self.simulation:
            self.simulation.close_simulation()



    def Draw(self):
        print("111")
        startx ,starty = self.Draw_target()
        traj = self.robotEnv.test()

        x,y= [],[]
        xb,yb = [],[]
        for i in traj:
            x.append(i[0])
            y.append(i[1])

        for i in self.robotEnv.ob:
            xb.append(i[0])
            yb.append(i[1])

        self.ui.ax1.clear()
        # tick_label后边跟x轴上的值，（可选选项：color后面跟柱型的颜色，width后边跟柱体的宽度）
        self.ui.ax1.plot(x,y)
        self.ui.ax1.scatter(xb,yb,c="blue",s =10)
        self.ui.ax1.scatter(self.robotEnv.goal[0],self.robotEnv.goal[1],c='red',s = 36)
        self.ui.ax1.scatter(startx,starty,c='green',s = 36)

        # self.ui.ax1.scatter(start_x,start_y,c = 'green')
        # self.ui.ax1.scatter(goal_x,goal_y,c = 'red')
   
        plt.title("MAP")
 
        # pos = self.ui.ax2.imshow(np.random.random((100, 100)), cmap=plt.cm.BuPu_r)
        # self.ui.figure.colorbar(pos, ax=self.ax2)      # 终于可以用colorbar了
 
        self.ui.canvas.draw()

    def Draw_obstac(self):
        self.robotEnv.reset()
        xb,yb = [],[]
        for i in self.robotEnv.ob:
            xb.append(i[0])
            yb.append(i[1])

        self.ui.ax1.clear()

        self.ui.ax1.scatter(xb,yb,c = '#0000FF',s=10)

        plt.title("MAP")
 
        self.ui.canvas.draw()

    def Draw_target(self):
        if (self.ui.lineEdit.text())and (self.ui.lineEdit_2.text()):
            start_x,start_y = int(self.ui.lineEdit.text()),int(self.ui.lineEdit_2.text())
        else:
            start_x,start_y = 0,0
        if (self.ui.lineEdit_2.text())and (self.ui.lineEdit_3.text()):
            goal_x,goal_y = int(self.ui.lineEdit_3.text()),int(self.ui.lineEdit_4.text()) 
        else:
            goal_x,goal_y =  10,10

        self.ui.ax1.scatter(start_x,start_y,c = 'green',s= 10)
        self.ui.ax1.scatter(goal_x,goal_y,c = 'red',s=10)
        plt.title("MAP")
 
        self.ui.canvas.draw()
        self.robotEnv.setstartandgoal([start_x,start_y,goal_x,goal_y])
        return start_x,start_y
    '''
    启动仿真环境并使用可以接受键盘控制指令
    '''
    def play(self):
        
        self.simulation.create_new_world()
        self.simulation.start_simu()
        self.simulation.close_simulation()

    '''
    自动载入必要的障碍物
    控制机器人以指定的速度移动
    完成运动
    '''
    def mov_the_robot_auto(self):
        self.simulation.create_world()
        obstacle = self.robotEnv.ob

        if (self.ui.lineEdit.text())and (self.ui.lineEdit_2.text()):
            start_x,start_y = int(self.ui.lineEdit.text()),int(self.ui.lineEdit_2.text())
        else:
            start_x,start_y = 0,0
        if (self.ui.lineEdit_2.text())and (self.ui.lineEdit_3.text()):
            goal_x,goal_y = int(self.ui.lineEdit_3.text()),int(self.ui.lineEdit_4.text()) 
        else:
            goal_x,goal_y =  10,10
        traj = self.robotEnv.test()
        v_w = []
        for state in traj:
            v_w.append([state[-2],state[-1]])
        self.simulation.start_auto(traj= v_w,obstacle = obstacle,start=[start_x,start_y])
if __name__ =="__main__":
    app = QApplication(sys.argv)
    # app.setWindowIcon(QIcon("./images/cartoon1.ico"))
    A = movWindow()

    A.show()
    
    sys.exit(app.exec_())